# AprilCalib log 5
# CalibRig::mode=2d
# @ Mon Nov 25 12:08:19 2019

from numpy import array
U=array([[533.9423828125, 491.7630310058594, 454.289306640625, 421.2277526855469, 537.7962646484375, 492.9359436035156, 453.6213989257813, 419.41162109375, 451.0653991699219, 491.1909484863281, 416.461181640625, 386.7468566894531, 537.8557739257813, 411.4047546386719, 445.9132995605469, 533.2772827148438, 486.4172058105469, 439.2486572265625, 478.6603698730469, 405.4554748535156, 524.0339965820313, 430.6142272949219, 468.2357482910156, 398.5975952148438, 364.8057250976563, 390.8991394042969, 510.9544677734375, 455.8352661132813, 420.8789367675781, 494.8450012207031],
       [31.51878356933594, 37.6710319519043, 44.96134948730469, 52.50407409667969, 85.01778411865234, 87.82102966308594, 91.4967041015625, 95.95896911621094, 142.7956695556641, 143.549560546875, 142.8908843994141, 143.0746917724609, 145.7534790039063, 192.6771240234375, 197.2270050048828, 210.9537353515625, 202.9738006591797, 252.0769653320313, 263.2811889648438, 242.9789886474609, 276.85986328125, 305.5598449707031, 321.5267333984375, 291.7584533691406, 323.19677734375, 338.1220703125, 339.93603515625, 375.2378845214844, 355.5830078125, 396.9832763671875]], dtype='float64');
Xw=array([[1299.504516601563, 1299.504516601563, 1299.504516601563, 1299.504516601563, 1099.504516601563, 1099.504516601563, 1099.504516601563, 1099.504516601563, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 99.50446319580078, 99.50446319580078, 299.5044555664063, 99.50446319580078, 99.50446319580078, 99.50446319580078],
       [899.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 899.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 499.5044555664063, 699.5044555664063, 299.5044555664063, 99.50446319580078, 899.5044555664063, 299.5044555664063, 499.5044555664063, 899.5044555664063, 699.5044555664063, 499.5044555664063, 699.5044555664063, 299.5044555664063, 899.5044555664063, 499.5044555664063, 699.5044555664063, 299.5044555664063, 99.50446319580078, 299.5044555664063, 899.5044555664063, 699.5044555664063, 499.5044555664063, 899.5044555664063],
       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype='float64');
# After LM:
K=array([[425.7098265737116, 0, 315.2829648257335],
       [0, 424.4149703294859, 250.7702411125528],
       [0, 0, 1]], dtype='float64');
distCoeffs=array([-0.375939271404359,
       0.1259779089452888,
       0.0007712594955163258,
       -0.01013106759212494,
       0], dtype='float64');
CovK=array([[0.06696221619623309, 0.06364166064710099, -0.02261295263704316, -0.01923311258503803, -0.0001536823451436975, 0.0002341991085222119, 9.051974296809816e-06, 1.328285376624064e-05, -9.959787472710305e-05],
       [0.06364166064710075, 0.06149676688783375, -0.02071883091607707, -0.01634041600124896, -0.0001457883735446724, 0.0002206097430468135, 8.002812245374864e-06, 1.255922963617716e-05, -9.360101004071655e-05],
       [-0.02261295263699026, -0.02071883091603567, 0.03718563433954366, 0.009706525634040963, 4.200213599411816e-05, -7.075878597865292e-05, -2.964220011109245e-06, -1.424397999584154e-05, 4.52329383439603e-05],
       [-0.01923311258515219, -0.01634041600135575, 0.009706525634276856, 0.03782854178989707, 4.041799412462779e-05, -6.027409664733702e-05, -1.261456020947368e-05, -5.429802752548048e-06, 1.132942148064637e-05],
       [-0.0001536823451437149, -0.0001457883735446284, 4.2002135994252e-05, 4.041799412439155e-05, 7.583014625657569e-07, -1.550012211247573e-06, -2.029539906941084e-08, -2.937756415407011e-08, 8.722318181777992e-07],
       [0.0002341991085222255, 0.0002206097430466689, -7.075878597884349e-05, -6.027409664691409e-05, -1.550012211247498e-06, 3.716408736419311e-06, 4.029591823164471e-08, 3.8621620271339e-08, -2.288456011356319e-06],
       [9.051974296847499e-06, 8.002812245410111e-06, -2.964220011185793e-06, -1.261456020948031e-05, -2.029539906949532e-08, 4.029591823179866e-08, 6.316661842823688e-09, 1.876986418188543e-09, -1.417156384646796e-08],
       [1.328285376622659e-05, 1.255922963616675e-05, -1.424397999585572e-05, -5.42980275246429e-06, -2.937756415401462e-08, 3.862162027123909e-08, 1.876986418161248e-09, 9.10458965777257e-09, -2.084303098898326e-08],
       [-9.95978747270309e-05, -9.360101004054947e-05, 4.52329383439312e-05, 1.132942148033223e-05, 8.722318181775832e-07, -2.288456011356048e-06, -1.417156384635952e-08, -2.084303098899191e-08, 1.503351468955703e-06]], dtype='float64');
# rms=0.341024
r0=array([0.1588564678193481,
       0.1648213224229561,
       -1.951038790807141], dtype='float64');
t0=array([-631.9911284370406,
       709.7514331786392,
       1972.910388691011], dtype='float64');
Covr0=array([[4.305026244459527e-07, -3.87021426429265e-08, 2.581554069591755e-08],
       [-3.870214264234545e-08, 2.94491129193443e-07, 3.53510130348966e-08],
       [2.581554069596582e-08, 3.535101303480437e-08, 1.303371291966197e-08]], dtype='float64');
Covt0=array([[0.8053496092370356, 0.2309189711299119, 0.4820791784612308],
       [0.2309189711349859, 0.800704251537213, 0.1243809474369623],
       [0.4820791784604367, 0.1243809474342622, 1.304436953841059]], dtype='float64');
r1=array([-0.1679391945251481,
       0.06974229416844775,
       -0.03333501643759839], dtype='float64');
t1=array([-636.5325244454946,
       -578.4327702655463,
       1787.365398067312], dtype='float64');
Covr1=array([[2.102479684027182e-07, -1.802876867864529e-08, -9.418062363971505e-09],
       [-1.802876867812162e-08, 1.688507673830048e-07, 1.676176248532555e-08],
       [-9.418062363891686e-09, 1.67617624853291e-08, 5.201804948324031e-09]], dtype='float64');
Covt1=array([[0.6477244760076216, 0.16070303794077, 0.5277956364381905],
       [0.1607030379448156, 0.6421010458263071, 0.4462527299500175],
       [0.5277956364396025, 0.4462527299471818, 1.32949584471569]], dtype='float64');
r2=array([-0.1171799227563381,
       -0.05953930241368506,
       0.6869201051553473], dtype='float64');
t2=array([-131.2345496902837,
       -1134.666146334156,
       1639.783060841854], dtype='float64');
Covr2=array([[2.210149181135178e-07, -1.74290739963327e-09, -9.131937258710626e-09],
       [-1.74290739893697e-09, 1.879144661431882e-07, 2.320112585654602e-08],
       [-9.131937258618578e-09, 2.320112585657847e-08, 6.676152031627633e-09]], dtype='float64');
Covt2=array([[0.5852885389346003, 0.1468300744865195, 0.4070418613983343],
       [0.1468300744901428, 0.568940245655241, 0.5176082592734105],
       [0.4070418614007535, 0.5176082592715779, 1.284476384170942]], dtype='float64');
r3=array([-0.2790772163032375,
       -0.002557592532003592,
       0.06207644185663701], dtype='float64');
t3=array([-730.7987839942758,
       -622.6325536115153,
       1746.150785583484], dtype='float64');
Covr3=array([[2.013049325567787e-07, -1.642019939159952e-08, -8.543048094006024e-09],
       [-1.642019939085198e-08, 1.570996093541693e-07, 2.2634666665229e-08],
       [-8.543048093877518e-09, 2.263466666523288e-08, 6.866693528484134e-09]], dtype='float64');
Covt3=array([[0.6114641949930896, 0.153186282049651, 0.5242103551183426],
       [0.153186282053578, 0.6194146158363872, 0.4647720260833255],
       [0.5242103551199401, 0.4647720260804838, 1.30424525067408]], dtype='float64');
r4=array([-0.4472863630849974,
       0.3289432773138485,
       -1.415244487149173], dtype='float64');
t4=array([-537.451821278852,
       705.2041095354006,
       1733.059259368653], dtype='float64');
Covr4=array([[3.022840706129226e-07, -4.45684246371384e-08, 2.875056730338904e-09],
       [-4.456842463610811e-08, 1.537447004394584e-07, 3.670459104519213e-08],
       [2.875056730665218e-09, 3.670459104512827e-08, 1.372394360850775e-08]], dtype='float64');
Covt4=array([[0.6229875593694254, 0.1803464741241925, 0.4333727783960993],
       [0.1803464741280477, 0.6205489739443235, 0.1100336353406604],
       [0.4333727783955178, 0.1100336353381909, 0.9220733256269845]], dtype='float64');
r5=array([-0.7676830887202628,
       0.8834870931011011,
       -1.223731026764929], dtype='float64');
t5=array([166.4860375861969,
       404.2831785926084,
       1978.609122652771], dtype='float64');
Covr5=array([[2.831978431499357e-07, -1.963107655913214e-08, 2.14112709545102e-08],
       [-1.963107655812174e-08, 1.495233889681723e-07, 6.670717747794912e-08],
       [2.141127095500533e-08, 6.670717747777331e-08, 4.862585020756748e-08]], dtype='float64');
Covt5=array([[0.7819016568490166, 0.1840137545466485, 0.2742023508075385],
       [0.1840137545515646, 0.7922024209979927, 0.116685100835976],
       [0.2742023508073793, 0.1166851008342753, 0.6042599930221892]], dtype='float64');
